#include "OakSensorVisual.h"

#include <iostream>
#include <vector>
#include <QtCore\qstring.h>
#include <QtCore\qdebug.h>
using namespace std;

OakSensorVisual::OakSensorVisual(const string &_serialNumber) {

	Oak_FindSensor(0x0000,0x0000,_serialNumber.data(),NULL,NULL,NULL,NULL,&m_sensor);
	m_handle = CreateFile(m_sensor.DevicePath,GENERIC_READ | GENERIC_WRITE,FILE_SHARE_READ | FILE_SHARE_WRITE,NULL,OPEN_EXISTING,FILE_ATTRIBUTE_NORMAL,NULL);
	m_iChannels = m_sensor.NumChannels;
	m_pBuffers = new CircularBuffer[m_iChannels]();

	//Configure report mode (after sampling)(default)
	//Configure led mode (blink)
	setLedMode(BLINKFAST);
	//Configure sample rate (10 ms)
	setSampleRateMs(0x0A);
}


OakSensorVisual::~OakSensorVisual(void)
{
	CloseHandle(m_handle);
}

bool OakSensorVisual::startCapture()
{
    m_bContinue = true;

	start();
    return true;
}

void OakSensorVisual::stop()
{
    m_bContinue = false;
}

void OakSensorVisual::run()
{
    if (m_handle == INVALID_HANDLE_VALUE){
        return;
	}

	static signed char buffer[1024]; 
	static DWORD numberReaded;
	vector<int> values;
	if(4 != values.size()) {
		values.resize(4);   
	}
    while (m_bContinue){
        // Read a complete value set from sensor
		ReadFile(m_handle,&buffer,256,&numberReaded,NULL);
		if(numberReaded==9){//Need to read 9 bytes for accelerometer
			for (unsigned int i =0; i < 4; i++){
				if(i!=0){
					values.at(i) =(buffer[1+2*i]+256*buffer[2+2*i]);
					//qDebug()<< QString("Chan%1: %2").arg(i).arg((buffer[1+2*i]+256*buffer[2+2*i]));
				}else{
					values.at(i) =  (unsigned char)buffer[1+2*i]+256*buffer[2+2*i];
					//qDebug()<< QString("Chan%1: %2").arg(i).arg((unsigned char)buffer[1+2*i]+256*buffer[2+2*i]);
				}
			}
		//	qDebug()<< QString("Chan1: %1,Chan2: %2,Chan3: %3,Chan4: %4").arg(values[0]*0.001).arg(values[1]*0.001).arg(values[2]*0.001).arg(values[3]*0.001);
			for (unsigned int i=0; i<values.size(); i++){
				m_pBuffers[i].append(values[i]);
			}
		}
        Sleep(1); // Used to limit the processor load
    };
}
void OakSensorVisual::setLedMode(LedModeEnum _newMode){
	BYTE inReport[33];
	inReport[0] = 0x00; //Always 0
	inReport[1] = 0x00; // 0 To set, 1 To get
	inReport[2] = 0x00; // 0 To ram, 1 To Flash
	inReport[3] = 0x01; //Number of data bytes
	inReport[4] = 0x01; //Index LSB
	inReport[5] = 0x00; //Index MSB
	inReport[6] = _newMode; //6-32 Datas

	Oak_Feature(m_sensor.DevicePath,inReport,false);

}
void OakSensorVisual::setReportMode(ReportModeEnum _newMode){
	BYTE inReport[33];
	inReport[0] = 0x00; //Always 0
	inReport[1] = 0x00; // 0 To set, 1 To get
	inReport[2] = 0x00; // 0 To ram, 1 To Flash
	inReport[3] = 0x01; //Number of data bytes
	inReport[4] = 0x00; //Index LSB
	inReport[5] = 0x00; //Index MSB
	inReport[6] = _newMode; //6-32 Datas

	Oak_Feature(m_sensor.DevicePath,inReport,false);
}
void OakSensorVisual::setSampleRateMs(int _rateMs){
	BYTE inReport[33];
	inReport[0] = 0x00; //Always 0
	inReport[1] = 0x00; // 0 To set, 1 To get
	inReport[2] = 0x00; // 0 To ram, 1 To Flash
	inReport[3] = 0x02; //Number of data bytes
	inReport[4] = 0x01; //Index LSB
	inReport[5] = 0x00; //Index MSB
	inReport[6] = _rateMs&0x0F; //6-32 Datas
	inReport[7] = (_rateMs&0xF0)>>8; //6-32 Datas

	Oak_Feature(m_sensor.DevicePath,inReport,false);
}
void OakSensorVisual::setReportRate(int _rateMs){
	BYTE inReport[33];
	inReport[0] = 0x00; //Always 0
	inReport[1] = 0x00; // 0 To set, 1 To get
	inReport[2] = 0x00; // 0 To ram, 1 To Flash
	inReport[3] = 0x02; //Number of data bytes
	inReport[4] = 0x00; //Index LSB
	inReport[5] = 0x00; //Index MSB
	inReport[6] = _rateMs&0x0F; //6-32 Datas
	inReport[7] = _rateMs&0xF0; //6-32 Datas

	Oak_Feature(m_sensor.DevicePath,inReport,false);
}

//*************************************************************************************************
// Function getBufferArray
/// @param channel The sensors channel
/// @param array A pointer to the array that will be filled with the sensor data
/// @param size The maximal size of the array
/// @param newOnly True for getting only the last values that have not been read since last call
/// @return The number of values written in the array
//*************************************************************************************************
int OakSensorVisual::getBufferArray(int channel, short* array, int size, bool newOnly)
{
    if (channel >= m_iChannels || channel < 0)
    {
        cerr << "Error on void UsbSensor::getBufferArray : invalid channel" << endl;
        return 0;
    }

    return m_pBuffers[channel].toArray(array, size, newOnly);
}